#include "algorithmservice.h"
#include "imagestoragemanager.h"
#include <AlgManager/pathdataparser.h>
#include <QFileDialog>
#include <QMessageBox>
#include <QDebug>
#include <QEventLoop>
#include <QJsonDocument>
#include <QJsonArray>
#include <QJsonObject>
#include <TaskManager.h>

// ProcessingThread 实现
ProcessingThread::ProcessingThread(AlgorithmService* algorithmService, QObject *parent)
    : QThread(parent)
    , m_algorithmService(algorithmService)
{
}

ProcessingThread::~ProcessingThread()
{
    // 确保线程安全退出
    if (isRunning()) {
        quit();
        wait(2000); // 等待2秒
    }
}

void ProcessingThread::setProcessingData(const s_Image3dS &image)
{
    m_image = image;

}

void ProcessingThread::run()
{
    try {
        // 在线程内部创建算法对象，避免与主线程共享
        CPreProcess preProcess;
        CPlateB plateB;
        SysEnv& env = SysEnv::instance();
        const s_PreProcess3DSPara& m_preProcessPara = env.cfg().preProcessPara;
        const s_DefectPlateBPara& m_defectPara = env.cfg().defectPara;
        const s_JggyPara& m_pathPara = env.cfg().pathPara;
        const s_PoseH& m_sPoseImage2Robot  = env.cfg().cameraToRobotPose;

        // 检查是否应该退出
        if (isInterruptionRequested()) {
            return;
        }

        // 步骤0: 根据相机连接状态决定拍照行为
        if (m_algorithmService && m_algorithmService->hasCamera()) {
            // 相机已连接：必须重新拍照
            emit progressUpdated(0, "开始单帧拍照...");

            if (!m_algorithmService->captureImage()) {
                emit processingFinished(false, "单帧拍照失败");
                return;
            }

            // 获取最新的图像数据
            m_image = m_algorithmService->getCurrentImage();
            // 检查是否应该退出
            if (isInterruptionRequested()) {
                return;
            }
        } else {
            // 相机未连接：检查是否有图像
            if (!m_algorithmService->hasImage()) {
                emit processingFinished(false, "相机未连接且没有可处理的图像数据");
                return;
            }
            // 相机未连接但有图像：直接使用现有图像，不拍照
            emit progressUpdated(0, "使用已加载的图像进行处理...");
        }

        // 步骤1: 预处理
        emit progressUpdated(1, "开始预处理...");
        s_PreProcess3DSResultPara preProcessResult;
        s_Rtnf result = preProcess.OnPreProcess(m_image, m_preProcessPara, preProcessResult);

        if (isInterruptionRequested()) {
            return;
        }

        if (result.iCode != 0) {
            emit processingFinished(false, QString("预处理失败: %1").arg(QString::fromStdString(result.strInfo)));
            return;
        }

        // 发送预处理完成信号，更新图像
        emit preProcessFinished(preProcessResult);

        // 检查是否应该退出
        if (isInterruptionRequested()) {
            return;
        }

        // 步骤2: 缺陷检测
        emit progressUpdated(2, "开始缺陷检测...");
        s_DefectPlateBRtsPara defectResult;
        result = plateB.OnDetect(preProcessResult.sImage3dPro, m_defectPara, defectResult);
        ImageStorageManager::getInstance().addDefectPlateTask(defectResult, "rts");
        if (isInterruptionRequested()) {
            return;
        }

        if (result.iCode != 0) {
            emit processingFinished(false, QString("缺陷检测失败: %1").arg(QString::fromStdString(result.strInfo)));
            return;
        }

        // 发送缺陷检测完成信号，更新图像
        emit defectDetectionFinished(defectResult);

        // 检查是否应该退出
        if (isInterruptionRequested()) {
            return;
        }

        // 步骤3: 路径规划
        emit progressUpdated(3, "开始路径规划...");

        s_LzppRtsPara pathResult;
        result = plateB.OnLzPP(m_pathPara, defectResult, m_sPoseImage2Robot, pathResult);
        // 保存到 JSON 文件
        m_algorithmService->savePathResultToJson(pathResult,defectResult, "D:/SaveData/alg_result.json");

        if (isInterruptionRequested()) {
            return;
        }

        if (result.iCode != 0) {
            emit processingFinished(false, QString("路径规划失败: %1").arg(QString::fromStdString(result.strInfo)));
            return;
        }

        // 发送路径规划完成信号，更新图像
        emit pathPlanningFinished(pathResult);

        if (isInterruptionRequested()) {
            return;
        }

        emit progressUpdated(4, "处理完成");
        emit processingFinished(true, "所有处理步骤完成");

    } catch (const std::exception &e) {
        if (!isInterruptionRequested()) {
            emit processingFinished(false, QString("处理异常: %1").arg(e.what()));
        }
    }
}

// 修改构造函数
AlgorithmService::AlgorithmService(ImageWindow* imageWindow, CameraService* cameraService, QObject* parent)
    : QObject(parent)
    , m_imageWindow(imageWindow)
    , m_cameraService(cameraService)
    , m_animationTimer(nullptr)
    , m_currentAnimationIndex(0)
    , m_animationTotalSteps(0)
{
    m_processingThread = new ProcessingThread(this);
    setupConnections();

    // 初始化Halcon设置
    SetHcppInterfaceStringEncodingIsUtf8(false);
    SetSystem("clip_region", "false");

    log("算法服务初始化完成");
}



AlgorithmService::~AlgorithmService()
{
    // 停止动画定时器
    if (m_animationTimer) {
        m_animationTimer->stop();
        delete m_animationTimer;
        m_animationTimer = nullptr;
    }

    // 停止处理线程
    if (m_processingThread && m_processingThread->isRunning()) {
        log("正在停止处理线程...");
        m_processingThread->quit();  // 温和地退出
        if (!m_processingThread->wait(3000)) { // 等待3秒
            log("处理线程未正常退出，强制终止");
            m_processingThread->terminate();
            m_processingThread->wait();
        }
    }

    delete m_processingThread;
    log("算法服务已销毁");
}

// 添加相机状态检查方法
bool AlgorithmService::hasCamera() const
{
    return m_cameraService && m_cameraService->isCameraConnected();
}

void AlgorithmService::setCameraService(CameraService *cameraService)
{
    m_cameraService = cameraService;
}

bool AlgorithmService::captureImage()
{
    if (!m_cameraService) {
        log("❌ 相机服务未设置，无法拍照");
        return false;
    }

    if (!m_cameraService->isCameraConnected()) {
        log("❌ 相机未连接，无法拍照");
        return false;
    }

    // 使用信号量或条件变量同步等待
    QEventLoop loop;
    QTimer timeoutTimer;
    timeoutTimer.setSingleShot(true);

    // 连接图像到达信号
    auto connection = connect(m_cameraService, &CameraService::frameAcquired,
                              &loop, &QEventLoop::quit);

    // 设置超时
    connect(&timeoutTimer, &QTimer::timeout, &loop, &QEventLoop::quit);
    timeoutTimer.start(5000); // 5秒超时

    // 触发单帧拍照
    m_cameraService->triggerSingleShot();

    // 等待图像到达或超时
    loop.exec();

    // 清理连接
    disconnect(connection);

    if (!timeoutTimer.isActive()) {
        log("❌ 拍照超时，未收到图像");
        return false;
    }

    log("✅ 单帧拍照完成");
    return true;
}

void AlgorithmService::stopPathAnimation()
{
    if (m_animationTimer && m_animationTimer->isActive()) {
        m_animationTimer->stop();
        log("路径动画已停止");
    }

    if (m_processingThread && m_processingThread->isRunning()) {
        log("正在停止算法处理线程...");
        m_processingThread->requestInterruption();
        m_processingThread->quit();
    }
}

void AlgorithmService::setupConnections()
{
    connect(m_processingThread, &ProcessingThread::processingFinished,
            this, &AlgorithmService::onProcessingFinished);
    connect(m_processingThread, &ProcessingThread::progressUpdated,
            this, &AlgorithmService::onProgressUpdated);
    connect(m_processingThread, &ProcessingThread::preProcessFinished,
            this, &AlgorithmService::onPreProcessFinished);
    connect(m_processingThread, &ProcessingThread::defectDetectionFinished,
            this, &AlgorithmService::onDefectDetectionFinished);
    connect(m_processingThread, &ProcessingThread::pathPlanningFinished,
            this, &AlgorithmService::onPathPlanningFinished);
}

void AlgorithmService::load3DImage(const QString& filePath)
{
    if (filePath.isEmpty()) {
        return;
    }

    try {
        QFileInfo fileInfo(filePath);
        QString basePath = fileInfo.absolutePath() + "/" + fileInfo.baseName();
        basePath = basePath.replace("_IMG_Texture_8Bit", "");

        log(QString("尝试从基础路径加载: %1").arg(basePath));
        log("检查必要的3D图像文件...");

        // 检查文件是否存在
        QStringList requiredFiles = {
            "_IMG_PointCloud_X.tif", "_IMG_PointCloud_Y.tif", "_IMG_PointCloud_Z.tif",
            "_IMG_NormalMap_X.tif", "_IMG_NormalMap_Y.tif", "_IMG_NormalMap_Z.tif"
        };

        bool allFilesExist = true;
        for (const QString& suffix : requiredFiles) {
            QString checkFile = basePath + suffix;
            if (!QFile::exists(checkFile)) {
                log(QString("❌ 文件不存在: %1").arg(suffix));
                allFilesExist = false;
            }
        }

        if (!allFilesExist) {
            return;
        }

        // 使用 CHVisionAdvX 加载3D图像
        s_Image3dS image;
        int result = m_visionAdv.ReadImage3DX(image, basePath.toStdString());

        if (result == 0) {
            m_currentImage = image;
            m_currentImage.ID = 1;

            m_imageWindow->ClearWindow();
            m_imageWindow->SetPart(0, 0, 2592, 1944);
            m_imageWindow->DispImage(m_currentImage.Gray);

            // 显示图像尺寸信息
            try {
                HTuple width, height;
                if (m_currentImage.Gray.IsInitialized()) {
                    GetImageSize(m_currentImage.Gray, &width, &height);
                    log(QString("图像尺寸: %1 x %2").arg(width.I()).arg(height.I()));
                }
            } catch (...) {
                log("图像尺寸信息获取失败");
            }

            clearResults();
            emit resultsUpdated();
            log("✅ 3D图像加载成功！现在可以进行预处理操作");

        } else {
            QString errorMsg;
            switch (result) {
            case 1: errorMsg = "灰度图像文件不存在"; break;
            case -1: errorMsg = "读取图像时发生异常"; break;
            default: errorMsg = QString("未知错误，代码: %1").arg(result); break;
            }
            log(QString("❌ 加载失败: %1").arg(errorMsg));
        }

    } catch (const std::exception &e) {
        QString error = QString("加载异常: %1").arg(e.what());
        log("❌ " + error);
    }
}

void AlgorithmService::preProcess()
{
    if (!hasImage()) {
        TaskManager::instance().completeTaskStatus(TaskType::MANUAL_PREPROCESS_ROI_CLUSTER,false,"请先加载图像");
        log("请先加载图像");
        return;
    }

    SysEnv& env = SysEnv::instance();
    const s_PreProcess3DSPara& prePara = env.cfg().preProcessPara;

    log("开始预处理...");
    s_PreProcess3DSResultPara sPreProcessRts;
    s_Rtnf result = m_preProcess.OnPreProcess(m_currentImage, prePara, sPreProcessRts);
    m_preProcessResult = sPreProcessRts.DeepCopy();

    if (result.iCode == 0) {
        m_imageWindow->ClearWindow();
        m_imageWindow->DispImage(sPreProcessRts.sImage3dPro.Gray.Clone());
        QString info = QString("预处理完成，耗时: %1 s").arg(sPreProcessRts.time, 0, 'f', 3);
        log(info);
        TaskManager::instance().completeTaskStatus(TaskType::MANUAL_PREPROCESS_ROI_CLUSTER,true,info);
        emit resultsUpdated();
    } else {
        QString error = QString("预处理失败: %1").arg(QString::fromStdString(result.strInfo));
        TaskManager::instance().completeTaskStatus(TaskType::MANUAL_PREPROCESS_ROI_CLUSTER,false,error);
        log(error);
    }
}

void AlgorithmService::defectDetection()
{
    if (!hasPreProcessResult()) {
        TaskManager::instance().completeTaskStatus(TaskType::MANUAL_DEFECT_DETECTION,false,"请先进行预处理");
        log("请先进行预处理");
        return;
    }

    SysEnv& env = SysEnv::instance();
    const s_DefectPlateBPara& defectPara = env.cfg().defectPara;

    log("开始缺陷检测...");
    s_DefectPlateBRtsPara sDefectPlateBRtsPara;
    s_Rtnf result = m_plateB.OnDetect(m_preProcessResult.sImage3dPro, defectPara, sDefectPlateBRtsPara);
    m_defectResult = sDefectPlateBRtsPara.DeepCopy();


    // ... 填充 defect_data ...
    ImageStorageManager::getInstance().addDefectPlateTask(sDefectPlateBRtsPara, "rts");

    if (result.iCode == 0) {
        // 显示缺陷检测结果
        m_imageWindow->ClearWindow();
        m_imageWindow->DispImage(sDefectPlateBRtsPara.ImageZ1ZeroReal.Clone());

        m_imageWindow->ClearObj();
        m_imageWindow->DispObj(sDefectPlateBRtsPara.RectPartLzC.Clone(), "red");
        m_imageWindow->DispObj(sDefectPlateBRtsPara.RectPartLzL.Clone(), "red");
        m_imageWindow->DispObj(sDefectPlateBRtsPara.RectPartLzU.Clone(), "red");
        m_imageWindow->DispObj(sDefectPlateBRtsPara.RectPartLzD.Clone(), "red");
        m_imageWindow->DispObj(sDefectPlateBRtsPara.RectPartLzR.Clone(), "red");

        QString info = QString("缺陷检测完成，耗时: %1 s").arg(m_defectResult.time, 0, 'f', 3);
        log(info);
        TaskManager::instance().completeTaskStatus(TaskType::MANUAL_DEFECT_DETECTION,true,info);
        emit resultsUpdated();
    } else {
        QString error = QString("缺陷检测失败: %1").arg(QString::fromStdString(result.strInfo));
        TaskManager::instance().completeTaskStatus(TaskType::MANUAL_DEFECT_DETECTION,false,error);
        log(error);
    }
}

void AlgorithmService::pathPlanning()
{
    if (!hasDefectResult()) {
        TaskManager::instance().completeTaskStatus(TaskType::MANUAL_DEFECT_DETECTION_SECONDARY,false,"请先进行缺陷检测");
        return;
    }

    SysEnv& env = SysEnv::instance();
    const s_JggyPara& pathPara = env.cfg().pathPara;
    const s_PoseH& posePara  = env.cfg().cameraToRobotPose;

    log("开始路径规划...");
    s_LzppRtsPara pathResult;
    s_Rtnf result = m_plateB.OnLzPP(pathPara, m_defectResult, posePara, pathResult);
    if(result.iCode == 0){
        parsePathPoint(pathResult);
        // 保存到 JSON 文件
        savePathResultToJson(pathResult, m_defectResult,"D:/SaveData/alg_result.json");
        TaskManager::instance().completeTaskStatus(TaskType::MANUAL_DEFECT_DETECTION_SECONDARY,true,"路径规划完成");
    }else{
        QString error = QString("路径规划失败: %1").arg(QString::fromStdString(result.strInfo));
        TaskManager::instance().completeTaskStatus(TaskType::MANUAL_DEFECT_DETECTION_SECONDARY,false,error);
        log(error);
    }

}

void AlgorithmService::parsePathPoint(s_LzppRtsPara pathResult)
{
    m_pathResult = pathResult.DeepCopy();
    // 解析路径数据
    m_commands.clear();
    m_commandGroups.clear();  // 清空分组

    PathDataParser parser;
    m_commands = parser.parsePathDataGrouped(m_pathResult);
    // 输出指令
    for (const QString& command : m_commands) {
        log("生成指令:" + command);
    }

    // 获取统计信息
    int totalPoints, totalCommands;
    parser.getParseStats(totalPoints, totalCommands);
    log(QString("解析统计: 总计%1点, 生成%2条指令").arg(totalPoints).arg(totalCommands));

    // 分组逻辑：按气缸变化进行分组
    groupCommandsByCylinder();
    // 显示路径规划结果
    HObject GrayXYZ;
    Compose4(m_preProcessResult.sImage3dPro.Gray,
             m_preProcessResult.sImage3dPro.X,
             m_preProcessResult.sImage3dPro.Y,
             m_preProcessResult.sImage3dPro.Z, &GrayXYZ);

    m_imageWindow->ClearWindow();
    m_imageWindow->DispImage(GrayXYZ);
    m_imageWindow->ClearObj();
    m_imageWindow->DispObj(m_pathResult.RegionMxAllList, "red");

    // 生成可视化图形
    generatePathVisualization();

    log(QString("路径规划完成，耗时: %1 s，生成 %2 个路径点")
            .arg(m_pathResult.time, 0, 'f', 3)
            .arg(m_pathResult.sListPPtsRobot.size()));

    // 输出路径点详细信息
    for (int i = 0; i < m_pathResult.sListPPtsRobot.size(); i++) {
        const s_PPts &point = m_pathResult.sListPPtsRobot[i];
        log(QString("路径点%1--def-xyz=[%2, %3, %4, %5]--图像坐标[%6, %7]--避让气缸索引=[%8]--最大深度=[%9]--当前深度=[%10]")
                .arg(i)
                .arg(point.iDef)
                .arg(point.fX, 0, 'f', 1)
                .arg(point.fY, 0, 'f', 1)
                .arg(point.fZ, 0, 'f', 1)
                .arg(point.fCol, 0, 'f', 1)
                .arg(point.fRow, 0, 'f', 1)
                .arg(QString::fromStdString(point.strQgNotSafe))
                .arg(point.ZMaxRelDm)
                .arg(point.MxHeightCur));
    }

    emit resultsUpdated();
}

void AlgorithmService::groupCommandsByCylinder()
{
    m_commandGroups.clear();
    QVector<QString> currentGroup;

    log("开始指令分组...");

    for (const QString& command : m_commands) {
        // 检测气缸指令 (CN=303 或 CN=304)
        if (command.contains("CN=303") || command.contains("CN=304")) {
            // 如果当前组不为空，先保存当前组
            if (!currentGroup.isEmpty()) {
                m_commandGroups.append(currentGroup);
                currentGroup.clear();
                log(QString("创建移动指令组，包含 %1 条指令").arg(m_commandGroups.last().size()));
            }

            // 气缸指令单独成组
            QVector<QString> cylinderGroup;
            cylinderGroup.append(command);
            m_commandGroups.append(cylinderGroup);
            log(QString("创建气缸指令组: %1").arg(command));
        }
        // 移动指令 (CN=210 或 CN=211)
        else if (command.contains("CN=210") || command.contains("CN=211")) {
            currentGroup.append(command);
        }
    }

    // 添加最后一组移动指令
    if (!currentGroup.isEmpty()) {
        m_commandGroups.append(currentGroup);
        log(QString("创建移动指令组，包含 %1 条指令").arg(currentGroup.size()));
    }

    log(QString("✅ 指令分组完成，共 %1 组").arg(m_commandGroups.size()));

    // 输出分组统计
    int totalGroupCommands = 0;
    for (int i = 0; i < m_commandGroups.size(); i++) {
        totalGroupCommands += m_commandGroups[i].size();
        log(QString("  组 %1: %2 条指令").arg(i + 1).arg(m_commandGroups[i].size()));
    }

    log(QString("分组验证: 总指令数 %1，分组后 %2").arg(m_commands.size()).arg(totalGroupCommands));
}

// 修改 runAllProcessing 方法
void AlgorithmService::runAllProcessing()
{
    // 检查处理条件
    if (hasCamera()) {
        log("全流程处理：相机已连接，将进行单帧拍照");
    } else if (hasImage()) {
        log("全流程处理：相机未连接，使用已加载的图像");
    } else {
        TaskManager::instance().completeTaskStatus(TaskType::CAPTURE,false,"全流程处理：相机未连接");
        return;
    }

    // 保存当前参数
    saveParameters();

    // 设置处理数据（如果有已加载的图像且相机未连接）
    if (hasImage() && !hasCamera()) {
        m_processingThread->setProcessingData(m_currentImage);
    }

    // 启动处理线程
    m_processingThread->start();

    log("开始全流程处理...");
}

void AlgorithmService::executePathAnimation()
{
    if (!hasPathResult()) {
        log("请先生成路径规划结果");
        return;
    }

    if (m_pathResult.sListPPtsRobot.empty()) {
        log("路径点为空，无法执行路径");
        return;
    }

    log("开始执行路径动画...");
    log(QString("路径总点数: %1").arg(m_pathResult.sListPPtsRobot.size()));

    // 创建合成图像用于显示
    HObject GrayXYZ;
    Compose4(m_preProcessResult.sImage3dPro.Gray,
             m_preProcessResult.sImage3dPro.X,
             m_preProcessResult.sImage3dPro.Y,
             m_preProcessResult.sImage3dPro.Z, &GrayXYZ);

    // 生成路径可视化对象（如果尚未生成）
    if (!m_pathCross.IsInitialized()) {
        generatePathVisualization();
    }

    // 清除窗口并显示基础图像
    m_imageWindow->ClearWindow();
    m_imageWindow->DispImage(GrayXYZ);

    // 显示静态元素
    m_imageWindow->ClearObj();
    m_imageWindow->DispObj(m_pathResult.RegionMxAllList, "blue");


    try
    {
        // 显示所有可视化元素
        m_imageWindow->DispObj(m_pathCross, "green");
        m_imageWindow->DispObj(m_pathPtsQgNotSafe, "red");
    }
    catch (HalconCpp::HException& ex)
    {

    }

    // 执行路径动画
    int totalArrows = 0;
    if (m_pathArrowList.IsInitialized()) {
        totalArrows = m_pathArrowList.CountObj();
    }

    log("路径动画开始执行...");

    // 使用成员变量来跟踪动画状态
    m_currentAnimationIndex = 0;
    m_animationTotalSteps = totalArrows;
    m_animationGrayXYZ = GrayXYZ;

    // 创建动画定时器
    if (m_animationTimer) {
        m_animationTimer->stop();
        delete m_animationTimer;
    }

    m_animationTimer = new QTimer(this);
    connect(m_animationTimer, &QTimer::timeout, this, &AlgorithmService::onAnimationTimeout);

    // 启动动画，每500ms更新一帧（比原来慢一些）
    m_animationTimer->start(500);
}

void AlgorithmService::onAnimationTimeout()
{
    if (m_currentAnimationIndex >= m_animationTotalSteps) {
        // 动画完成
        m_animationTimer->stop();
        onPathAnimationFinished(m_animationGrayXYZ);
        return;
    }

    HObject currentArrow;
    SelectObj(m_pathArrowList, &currentArrow, m_currentAnimationIndex + 1);

    // 计算进度百分比
    int progress = (m_currentAnimationIndex + 1) * 100 / m_animationTotalSteps;

    // 清除并重新显示基础内容
    m_imageWindow->ClearWindow();
    m_imageWindow->DispImage(m_animationGrayXYZ);
    m_imageWindow->ClearObj();

    // 显示静态元素
    m_imageWindow->DispObj(m_pathResult.RegionMxAllList, "blue");
    if (m_pathCross.IsInitialized()) {
        m_imageWindow->DispObj(m_pathCross, "green");
    }
    if (m_pathPtsQgNotSafe.IsInitialized()) {
        m_imageWindow->DispObj(m_pathPtsQgNotSafe, "red");
    }

    // 显示已执行的箭头（灰色）
    for (int i = 0; i < m_currentAnimationIndex; i++) {
        HObject previousArrow;
        SelectObj(m_pathArrowList, &previousArrow, i + 1);
        m_imageWindow->SetLineWidthX(2);
        m_imageWindow->DispObj(previousArrow, "gray");
    }

    // 高亮显示当前箭头
    m_imageWindow->SetLineWidthX(3);
    m_imageWindow->DispObj(currentArrow, "red");
    m_imageWindow->SetLineWidthX(2);

    // 更新日志显示当前路径点和进度
    if (m_currentAnimationIndex < m_pathResult.sListPPtsRobot.size() - 1) {
        const s_PPts &currentPoint = m_pathResult.sListPPtsRobot[m_currentAnimationIndex];
        const s_PPts &nextPoint = m_pathResult.sListPPtsRobot[m_currentAnimationIndex + 1];

        log(QString("进度 %1% 步骤 %2/%3: 从 [%4, %5, %6] 到 [%7, %8, %9]")
                .arg(progress)
                .arg(m_currentAnimationIndex + 1)
                .arg(m_animationTotalSteps)
                .arg(currentPoint.fX, 0, 'f', 1)
                .arg(currentPoint.fY, 0, 'f', 1)
                .arg(currentPoint.fZ, 0, 'f', 1)
                .arg(nextPoint.fX, 0, 'f', 1)
                .arg(nextPoint.fY, 0, 'f', 1)
                .arg(nextPoint.fZ, 0, 'f', 1));
    }

    m_currentAnimationIndex++;
}

void AlgorithmService::onPathAnimationFinished(const HObject& GrayXYZ)
{
    log("✅ 100% 路径动画执行完成!");
    log("✅ 所有路径点已遍历完成");

    // 统计不同类型路径点
    int startPoints = 0, endPoints = 0, wayPoints = 0, airPoints = 0;
    for (const auto &point : m_pathResult.sListPPtsRobot) {
        switch (point.iDef) {
        case 1: startPoints++; break;
        case 2: endPoints++; break;
        case 3: wayPoints++; break;
        case 4: airPoints++; break;
        }
    }

    log(QString("路径统计: 起点(%1) 终点(%2) 路径点(%3) 空移点(%4)")
            .arg(startPoints).arg(endPoints).arg(wayPoints).arg(airPoints));

    // 动画结束后保持最终状态显示
    m_imageWindow->ClearWindow();
    m_imageWindow->DispImage(GrayXYZ);
    m_imageWindow->ClearObj();
    m_imageWindow->DispObj(m_pathResult.RegionMxAllList, "blue");

    if (m_pathCross.IsInitialized()) {
        m_imageWindow->DispObj(m_pathCross, "green");
    }

    if (m_pathPtsQgNotSafe.IsInitialized()) {
        m_imageWindow->DispObj(m_pathPtsQgNotSafe, "red");
    }

    if (m_pathArrowList.IsInitialized()) {
        m_imageWindow->DispObj(m_pathArrowList, "red");
    }

    // 重置动画状态
    m_currentAnimationIndex = 0;
    m_animationTotalSteps = 0;
}

void AlgorithmService::saveParameters()
{
    SysEnv& env = SysEnv::instance();
    env.save();
    log("参数已保存");
}

void AlgorithmService::savePathResultToJson(const s_LzppRtsPara& pathResult,s_DefectPlateBRtsPara& defectResult, const QString& filePath)
{
    try {
        QFile file(filePath);

        // 如果文件不存在，创建目录
        QFileInfo fileInfo(file);
        QDir dir = fileInfo.absoluteDir();
        if (!dir.exists()) {
            dir.mkpath(".");
        }

        // 打开文件
        if (!file.open(QIODevice::WriteOnly | QIODevice::Text | QIODevice::Truncate)) {
            log(QString("❌ 无法打开文件进行写入: %1").arg(filePath));
            return;
        }

        // 创建 JSON 对象
        QJsonObject rootObject;

        // 基本信息
        rootObject["bTJG"] = pathResult.bTJG;
        rootObject["time"] = pathResult.time;
        rootObject["iNumPts"] = pathResult.iNumPts;

        // 图像坐标系下的路径点
        QJsonArray imagePointsArray;
        for (const auto& point : pathResult.sListPPtsImage) {
            QJsonObject pointObj;
            pointObj["fX"] = point.fX;
            pointObj["fY"] = point.fY;
            pointObj["fZ"] = point.fZ;
            pointObj["iDef"] = point.iDef;
            pointObj["fRow"] = point.fRow;
            pointObj["fCol"] = point.fCol;
            pointObj["strQgNotSafe"] = QString::fromStdString(point.strQgNotSafe);
            pointObj["ZMaxRelDm"] = point.ZMaxRelDm;
            pointObj["MxHeightCur"] = point.MxHeightCur;
            imagePointsArray.append(pointObj);
        }
        rootObject["sListPPtsImage"] = imagePointsArray;

        // 机器人坐标系下的路径点
        QJsonArray robotPointsArray;
        for (const auto& point : pathResult.sListPPtsRobot) {
            QJsonObject pointObj;
            pointObj["fX"] = point.fX;
            pointObj["fY"] = point.fY;
            pointObj["fZ"] = point.fZ;
            pointObj["iDef"] = point.iDef;
            pointObj["fRow"] = point.fRow;
            pointObj["fCol"] = point.fCol;
            pointObj["strQgNotSafe"] = QString::fromStdString(point.strQgNotSafe);
            pointObj["ZMaxRelDm"] = point.ZMaxRelDm;
            pointObj["MxHeightCur"] = point.MxHeightCur;
            robotPointsArray.append(pointObj);
        }
        rootObject["sListPPtsRobot"] = robotPointsArray;

        // 区域信息（简化处理，只记录区域是否存在）
        rootObject["hasRegionMxAllList"] = pathResult.RegionMxAllList.IsInitialized();

        // 缺陷检测结果数据
        QJsonObject defectResultObject;

        // 缺陷检测基本信息
        defectResultObject["bTJG"] = defectResult.bTJG;
        defectResultObject["time"] = defectResult.time;

        // 粒子数量
        defectResultObject["lzNum"] = defectResult.GetLzNum();

        // 粒子总面积
        defectResultObject["lzAreaTotalMM"] = defectResult.GetLzAreaTotalMM();

        // 粒子最大高度
        defectResultObject["lzHeightMax"] = defectResult.GetLzHeightMax();

        // 粒子总体积
        defectResultObject["lzVolumeTotal"] = defectResult.GetPartLzVolumeATotal();

        // 像素分辨率
        defectResultObject["resolution"] = defectResult.GetResolution();

        // 粒子块的矩形区域
        QJsonArray lzRectArray;
        std::vector<s_RectX> rects = defectResult.GetRectsPartLz();
        for (const auto& rect : rects) {
            QJsonObject rectObj;
            rectObj["X1"] = rect.X1;
            rectObj["Y1"] = rect.Y1;
            rectObj["X2"] = rect.X2;
            rectObj["Y2"] = rect.Y2;
            lzRectArray.append(rectObj);
        }
        defectResultObject["lzRects"] = lzRectArray;

        // 单个粒子的矩形区域
        QJsonArray singleLzRectArray;
        std::vector<s_RectX> singleRects = defectResult.GetRectsLz();
        for (const auto& rect : singleRects) {
            QJsonObject rectObj;
            rectObj["X1"] = rect.X1;
            rectObj["Y1"] = rect.Y1;
            rectObj["X2"] = rect.X2;
            rectObj["Y2"] = rect.Y2;
            singleLzRectArray.append(rectObj);
        }
        defectResultObject["singleLzRects"] = singleLzRectArray;

        // 将缺陷结果添加到根对象
        rootObject["defectResult"] = defectResultObject;

        // 从系统配置中获取相机到机器人的变换矩阵
        SysEnv& env = SysEnv::instance();
        s_PoseH cameraToRobotPose = env.cfg().cameraToRobotPose;
        std::vector<double> homMat3dValues = cameraToRobotPose.getHomMat3dValues();
        if (!homMat3dValues.empty()) {

            // 将变换矩阵保存为数组
            QJsonArray homMat3dArray;
            for (double value : homMat3dValues) {
                homMat3dArray.append(value);
            }
            rootObject["cameraToRobotHomMat3d"] = homMat3dArray;

            // 同时保存原始的位姿参数
            QJsonObject cameraToRobotPoseObj;
            cameraToRobotPoseObj["fTransX"] = cameraToRobotPose.fTransX;
            cameraToRobotPoseObj["fTransY"] = cameraToRobotPose.fTransY;
            cameraToRobotPoseObj["fTransZ"] = cameraToRobotPose.fTransZ;
            cameraToRobotPoseObj["fRotX"] = cameraToRobotPose.fRotX;
            cameraToRobotPoseObj["fRotY"] = cameraToRobotPose.fRotY;
            cameraToRobotPoseObj["fRotZ"] = cameraToRobotPose.fRotZ;
            cameraToRobotPoseObj["dwType"] = cameraToRobotPose.dwType;
            rootObject["cameraToRobotPose"] = cameraToRobotPoseObj;

            log(QString("✅ 相机到机器人变换矩阵已保存: %1 个元素").arg(homMat3dValues.size()));
        } else {
            log("⚠️ 相机到机器人位姿未配置，跳过保存变换矩阵");
        }

        // 创建 JSON 文档
        QJsonDocument jsonDoc(rootObject);

        // 写入文件
        file.write(jsonDoc.toJson(QJsonDocument::Indented));
        file.close();

        log(QString("✅ 路径规划结果已保存到: %1").arg(filePath));
        log(QString("   包含 %1 个图像坐标点，%2 个机器人坐标点")
                .arg(pathResult.sListPPtsImage.size())
                .arg(pathResult.sListPPtsRobot.size()));

        // 输出缺陷统计信息
        log(QString("   缺陷统计: 粒子数量=%1, 总面积=%2 mm², 最大高度=%3 mm, 总体积=%4 mm³")
                .arg(defectResult.GetLzNum())
                .arg(defectResult.GetLzAreaTotalMM(), 0, 'f', 3)
                .arg(defectResult.GetLzHeightMax(), 0, 'f', 3)
                .arg(defectResult.GetPartLzVolumeATotal(), 0, 'f', 3));

    } catch (const std::exception &e) {
        log(QString("❌ 保存JSON文件时发生异常: %1").arg(e.what()));
    }
}

void AlgorithmService::updateParameterDisplay()
{
    // 更新参数显示逻辑
    emit resultsUpdated();
}

void AlgorithmService::generatePathVisualization()
{
    // 生成加工点十字标记
    HTuple hRowAll, hColAll;
    HTuple hRowQg, hColQg;

    // 使用 sListPPtsRobot
    for (int i = 0; i < m_pathResult.sListPPtsRobot.size(); i++) {
        const s_PPts &point = m_pathResult.sListPPtsRobot[i];
        hRowAll.Append(point.fRow);
        hColAll.Append(point.fCol);

        if (!point.strQgNotSafe.empty()) {
            hRowQg.Append(point.fRow);
            hColQg.Append(point.fCol);
        }
    }

    // 加工点十字标记
    HObject hoCross;
    GenCrossContourXld(&hoCross, hRowAll, hColAll, 21, 0);

    // 避让点圆形标记
    HObject hoPtsQgNotSafe;
    HTuple hvRadius;

    try
    {
        int iNum = hRowQg.Length();
        if (iNum > 0)
        {
            TupleGenConst(HTuple(hRowQg.TupleLength()), 5, &hvRadius);
            GenCircle(&hoPtsQgNotSafe, hRowQg, hColQg, hvRadius);

        }
    }
    catch (HalconCpp::HException& ex)
    {

    }


    // 路径箭头 - 使用 visionAdv 的方法生成
    HObject hoArrowList;
    GenEmptyObj(&hoArrowList);
    for (int i = 0; i < m_pathResult.sListPPtsRobot.size() - 1; i++) {
        const s_PPts &currentPoint = m_pathResult.sListPPtsRobot[i];
        const s_PPts &nextPoint = m_pathResult.sListPPtsRobot[i + 1];

        HObject hoArrow;
        GenEmptyObj(&hoArrow);
        m_visionAdv.gen_arrow_contour_xld(&hoArrow,
                                          currentPoint.fRow, currentPoint.fCol,
                                          nextPoint.fRow, nextPoint.fCol, 5, 3);
        ConcatObj(hoArrowList, hoArrow, &hoArrowList);
    }

    // 保存可视化对象供后续使用
    m_pathCross = hoCross;
    m_pathPtsQgNotSafe = hoPtsQgNotSafe;
    m_pathArrowList = hoArrowList;

    log("路径可视化图形生成完成");
}

void AlgorithmService::displayResults()
{
    // 结果显示逻辑
    emit resultsUpdated();
}

void AlgorithmService::clearResults()
{
    m_preProcessResult = s_PreProcess3DSResultPara();
    m_defectResult = s_DefectPlateBRtsPara();
    m_pathResult = s_LzppRtsPara();
    m_commands.clear();
}

void AlgorithmService::log(const QString& message)
{
    qDebug() << "[AlgorithmService]" << message;
    emit logMessage(message);
}

// 处理线程信号槽
void AlgorithmService::onProcessingFinished(bool success, const QString& message)
{
    if (success) {
        log("全流程处理完成");
        TaskManager::instance().completeTaskStatus(TaskType::CAPTURE,success,"全流程处理完成");
    } else {
        QString erro = QString("处理失败: %1").arg(message);
        log(erro);
        TaskManager::instance().completeTaskStatus(TaskType::CAPTURE,success,erro);
    }
}

void AlgorithmService::onProgressUpdated(int step, const QString& status)
{
    log(status);
}

void AlgorithmService::onPreProcessFinished(const s_PreProcess3DSResultPara& result)
{
    m_preProcessResult = result;
    m_imageWindow->ClearWindow();
    m_imageWindow->DispImage(result.sImage3dPro.Gray.Clone());
    emit resultsUpdated();
    log(QString("预处理完成，耗时: %1 s").arg(result.time, 0, 'f', 3));
}

void AlgorithmService::onDefectDetectionFinished(const s_DefectPlateBRtsPara& result)
{
    m_defectResult = result;
    m_imageWindow->ClearWindow();
    m_imageWindow->DispImage(result.ImageZ1ZeroReal.Clone());

    m_imageWindow->ClearObj();
    m_imageWindow->DispObj(result.RectPartLzC.Clone(), "red");
    m_imageWindow->DispObj(result.RectPartLzL.Clone(), "red");
    m_imageWindow->DispObj(result.RectPartLzU.Clone(), "red");
    m_imageWindow->DispObj(result.RectPartLzD.Clone(), "red");
    m_imageWindow->DispObj(result.RectPartLzR.Clone(), "red");

    emit resultsUpdated();
    log(QString("缺陷检测完成，耗时: %1 s").arg(result.time, 0, 'f', 3));
}

void AlgorithmService::onPathPlanningFinished(const s_LzppRtsPara& result)
{
    parsePathPoint(result);
}

void AlgorithmService::onFrameAcquired(const s_Image3dS &frame)
{
    m_currentImage = frame;
    m_currentImage.ID = 1;
    // 添加存图任务
    ImageStorageManager::getInstance().addImage3dTask(frame, "src");
}
